#include "WPILib.h"
#include "Robot2489.h"

bool kicking = false;

/**
 * Runs the motors under driver control with either tank or arcade steering
 * selected by a jumper in DS Digin 0.
 *
 * Starts the compressor.
 *
 * Enables the Watchdog.
 *
 * Initializes Kicker and Latch to Relaxed and Closed respectively.
 *
 * Then starts the big Operator Control while loop:
 * 		Drives the robot using Tank Drive and the joysticks connected to ports
 * 		1 and 2.
 */

void Robot2489::OperatorControl()
{
	bool button; //the left stick's trigger's state, pressed or not
	bool kickerReadying = true;
	bool latchOpened = false;
	double timerState;

	/*
	 * Camera stuff; look into later
	 *
	 * AxisCamera &camera = AxisCamera::GetInstance();
	 * camera.WriteResolution(AxisCamera::kResolution_320x240);
	 * camera.WriteCompression(20);
	 * camera.WriteBrightness(0);
	 */

	KarlKompressor->Start();
	GetWatchdog().SetEnabled(true);

	CloseLatch();
	ChargeKicker();

	while (IsOperatorControl())
	{
		myRobot->SetLeftRightMotorSpeeds(MotorControl(leftStick->GetY()),
				MotorControl(rightStick->GetY()));

		if (button = manipStick->GetTrigger())
		{
			kickTimar->Start();
			kicking = true;
		}

		timerState = kickTimar->Get();

		// Make sure that the latch is opened during kick.
		if (timerState < LATCHOPENTIME && !latchOpened)
		{
			OpenLatch();
			latchOpened = true;
		}
		// Make sure that the kicker is retracted after kick.
		if (timerState >= LATCHOPENTIME && kickerReadying)
		{
			RetractKicker();
			kickerReadying = false;
		}
		// Make sure that the latch closes when the kicker's retracted.
		if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME) && latchOpened)
		{
			CloseLatch();
			latchOpened = false;
		}
		// Make sure that the kicker begins readying when restored.
		if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME)
				&& !kickerReadying)
		{
			ChargeKicker();
			kickerReadying = true;
		}
		// When everything's in place and charging, reset timer.
		if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME
				+ KICKERREADYINGTIME))
		{
			kickTimar->Stop();
			kickTimar->Reset();
		}
		GetWatchdog().Feed();
	}
}
